//
// Created by pulsarv on 21-1-29.
//

#ifndef ROBOCAR_DATA_STRUCT_H
#define ROBOCAR_DATA_STRUCT_H

#include <string>
#include <mutex>
#include <map>
#include <opencv2/opencv.hpp>

namespace rccore {
    namespace data_struct {
        typedef int COMMAND_TYPE;
        typedef int ACTION_TYPE;

//命令类型
        enum {
            NONE_COMMAND = 0,//空命令
            MOVE_COMMAND,//移动命令
            SWITCH_COMMAND//开关命令 此命令传入bool类型值
        };

//移动指令
        enum {
            MOVE_STOP = 0, //动作停止
            MOVE_AC, //逆时针旋转
            MOVE_CW, // 顺时针旋转
            MOVE_BACK, // 后退
            MOVE_FRONT, // 前进
            MOVE_LEFT, // 左飞
            MOVE_RIGHT, // 右飞
            MOVE_UP, // 爬升
            MOVE_DOWN, // 下降
            TAKE_OFF // 起飞
        };

// TODO:状态机值
        enum {
            FOLLOW = 0, //跟随模式
            LAND, // 降落模式
            HOVER, // 覆盖模式（即发现了TAG）
            NORMAL, // 普通模式（啥都不做）
            LOSS_HOVER // 丢失TAG以后的状态
        };

//命令包
        class command_t {
        public:
            COMMAND_TYPE type = MOVE_COMMAND;//命令类型 默认移动控制
            ACTION_TYPE actionType = MOVE_STOP;//命令类型 默认停止
            float value = 0.0;//命令浮点类型值
            int int_value = 0;//命令整形值
            bool bool_value = false;//命令布尔类型值
            std::string string_value;//命令字符串类型值
        };

//TAG
        struct Apriltag {
            int id = 0;//TAG ID号
            int size = 150;//TAG 大小
        };

//配置信息
        struct ConfigInfo {
            // TODO:摄像头配置参数
            int camera_index = 0; // 摄像头下标
            int resize_width = 640; // 摄像头下标
            int resize_height = 640; // 摄像头下标
            float camera_detcet_size = 1.0f; // 摄像头下标
            bool save_video = false; // 摄像头下标
            int camera_fps = 30; // 摄像头下标
            std::string camera_info = "CameraInfo.xml"; // 摄像头标定文件 默认值：CameraInfo.xml

            //TODO:Lidar
            std::string lidar_device_path = ""; //模拟器开关

            // TODO:Network配置参数
            bool HTTPD_ON = true;
            int HTTPD_PORT = 20800;

            // TODO:EKF配置参数
            float ekf_p = 0.1f;
            float ekf_q = 0.1f;
            float ekf_r = 0.1f;
            float DELTA_T = 2.0f;
            //状态转移矩阵
            cv::Mat transitionMatrix = (cv::Mat_<float>(18, 18)
                    << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0, 0, 0, 0, 0,
                    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0, 0, 0, 0,
                    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0.5 * DELTA_T * DELTA_T,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1
            );
            size_t MAX_POINTS_SIZE = 100;
            bool EKF_ON = false;

            // TODO:系统配置参数
            bool debug_window = false; // 窗口开关
            bool AUTO_CONTROL = false; // 窗口开关
            float LANDED_ALTITUDE = 0.09f; // 着陆高度
            float FOLLOW_ALTITUDE = 8.0f; // 跟随高度
            float TAKEOFF_ALTITUDE = 3.0f; // 起飞高度
            int LOSS_HOVER_TIMEOUT_TIME = 1000; // 起飞高度

            // TODO:运动控制参数
            bool LAND_ON = false; // 降落开关
            float LAND_DISTANCE = 10.0f; // 前后逼近距离
            float MIN_LAND_ALTITUDE = 0.3f; // 最小降落距离

            bool FORWARD_ON = false; // 前后自稳开关
            float FORWARD_MAX_V_M_S = 1.0f; // 前后
            float FORWARD_P = 0.002f;
            float FORWARD_I = 0.002f;
            float FORWARD_D = 0.002f;

            bool ROTATE_ON = false; // 旋转自稳开关
            float ROTATE_MAX_V_M_S = 1.0f; // 前后
            float ROTATE_P = 0.002f;
            float ROTATE_I = 0.002f;
            float ROTATE_D = 0.002f;

            bool RIGHT_ON = false; // 左右自稳开关
            float RIGHT_MAX_V_M_S = 1.0f; // 前后
            float RIGHT_P = 0.002f;
            float RIGHT_I = 0.002f;
            float RIGHT_D = 0.002f;


            std::map<int, Apriltag> Apriltags;
        };
//系统信息
        struct SystemInfo {
            float Altitude_m = 100.0;
            float absolute_altitude_m = 0.0;

            float yawspeed_deg_s = 0.0;
            float forward_m_s = 0.0;
            float right_m_s = 0.0;

            float EKF_ACTION_X_M_S = 0.0;
            float EKF_ACTION_Y_M_S = 0.0;
            float EKF_ACTION_Z_M_S = 0.0;

            float latitude_deg = 0.0;
            float longitude_deg = 0.0;

            float EKF_LAT = 0.0;
            float EKF_LNG = 0.0;

            float velocity_x_m_s = 0.0;
            float velocity_y_m_s = 0.0;
            float velocity_z_m_s = 0.0;

            float EKF_VX_M_S = 0.0;
            float EKF_VY_M_S = 0.0;
            float EKF_VZ_M_S = 0.0;

            float TRANS_X = 0.0;
            float TRANS_Y = 0.0;
            float TRANS_Z = 0.0;

            float EKF_TX = 0.0;
            float EKF_TY = 0.0;
            float EKF_TZ = 0.0;

            float EULAR_X = 0.0;
            float EULAR_Y = 0.0;
            float EULAR_Z = 0.0;


            float EKF_EX = 0.0;
            float EKF_EY = 0.0;
            float EKF_EZ = 0.0;

            float DISTANCE = 0.0;
            std::string s_flight_mode;
        };
    }
}


#endif // ROBOCAR_DATA_STRUCT_H
